/******************************************************************************
** 
** File name:	rtos_timer.S
**	
** Description:	Timer Management. This file contains implementation of 
**				basic RTOS services realted to timer queue managment 
**				functionality (includes RTOS_WaitTimer).
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**	void 			RTOS_Delay				( RTOS_TMR_t *  , RTOS_U16_t )
**	void 			RTOS_StartTimer			( RTOS_TMR_t *  , RTOS_U16_t )
**	void 			RTOS_StartTimerMessage	( RTOS_TMS_t *  , RTOS_U16_t, RTOS_MSQ_t * )
**	RTOS_TMR_t *	RTOS_CancelTimer		( RTOS_TMR_t * )
**	RTOS_TMS_t *	RTOS_CancelTimerMessage	( RTOS_TMS_t *  , RTOS_MSQ_t * )
**	void 			RTOS_TimerHandlerFNC	( void )
**	RTOS_U16_t		RTOS_GetTime			( void )
**
** Notes:		This is one of two versions for the RTOS timer 
**				functionality. 
**
*******************************************************************************
** History:
**
**	10/15/2011 - AK - Moving to RTOS Ver.2. First prototype version.
** 	11/20/2011 - AK - Adding RTOS_CancelTimer()
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					  with MPP binary (Dec 8, 2011).
**	09/20/2012 - AK - RTOS v3. _ENTER_CRITICAL/_LEAVE_CRITICAL 
**							   _RTOS_PROLOG/_RTOS_EPILOG
**	10/09/2012 - AK - added RTOS_StartTimerMessage(), RTOS_CancelTimerMessage()
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_timer)
		
	_EXTERN(RTOS_SendMessageISR)
	_EXTERN(RTOS_SetObjectEventISR)
	_EXTERN(RTOS_WaitTimer)
    _EXTERN(_EnterISR)
	_EXTERN(_LeaveISR)
    _EXTERN(_RemoveObj)
 		
        _DATASECTION

	_GLOBAL(_RtosTimerQueue,   2)       ;; Head of the timer queue
	_GLOBAL(_RtosTimeCounter,  2)       ;; Simple counter (incremented in every timer interrupt)	

        _CODESECTION
		
/******************************************************************************
;; Prototype:	void RTOS_Delay(RTOS_TMR_t *  , RTOS_U16_t )
;; Description:	This service implement user level delay functionality. The 
;;				difference with regular libc delay() is that RTOS will let 
;;				other tasks do their job until delay() caller is waiting.
;; Passed:      p1h:p1l = &(tmr) - pointer to Timer 
;;              p2h:p2l = count  - 16-bits delay in RTOS system ticks (see RTOS_TimerHandler)
;; Returns:		none
;; Uses:        everithing )
;; Stack:		TBD
;; Calls:		RTOS_StartTimer, RTOS_WaitTimer
;; Assumes:		-
;; Notes:		Actually same functionality can be implemented by sequencial
;;				call of RTOS_StartTimer()/RTOS_WaitTaimer()
******************************************************************************/
	_FUNCTION(RTOS_Delay)
        
RTOS_Delay:

	rcall   RTOS_StartTimer
	rjmp    RTOS_WaitTimer

	_ENDFUNC	;; _FUNCTION(RTOS_Delay)

/******************************************************************************
;; Prototype:	void RTOS_StartTimer(RTOS_TMR_t *  tmr, RTOS_U16_t )
;; Description:	Should check and halt if TCB already queued. Resets timer
;;				object event if counter==0 and returns	
;; Passed:      p1h:p1l = &tmr  - pointer to Timer 
;;              p2h:p2l = count - 16-bits delay in RTOS system ticks (see 
;;				RTOS_TimerHandler interrupt handler)
;; Returns:		none
;; Uses:        Z, Y, tmp0, tmp1
;; Stack:		TBD
;; Calls:		RTOS_SetObjectEvent
;; Assumes:		-
;; Notes:		This functions should check if timer is already queued. If 
;;				timer' counter is zero then timer' event shold be reseted. 
******************************************************************************/
	_FUNCTION(RTOS_StartTimer)
        
RTOS_StartTimer:

	subi    p2l, lo8(-0)
	sbci    p2h, hi8(-0)
	
	brne    CountNotZero
	
	rjmp    RTOS_SetObjectEvent

	;;_PUBLIC(CountNotZero)

CountNotZero:        

	;;rcall 	_EnterISR					; RTOS_Prolog
	
	_ENTER_CRITICAL
	
	_RTOS_PROLOG

    rcall   RTOS_ResetObjectEventISR

	ldi     Zl, lo8(_RtosTimerQueue)
	ldi     Zh, hi8(_RtosTimerQueue)

	_ENTER_CRITICAL

ast00:

	mov     Yl, Zl          			; Y -> Previous
	mov     Yh, Zh
	ldd     Zl, Y+NextL     			; Z -> Current
	ldd     Zh, Y+NextH
	adiw    Zl, 0
	breq    ast01           			; End of queue, Wrap up.

	ldd     tmp0, Z+TmrCount+NextL		; tmp = Current count
	ldd     tmp1, Z+TmrCount+NextH		; p2 = our count

	sub     p2l, tmp0
	sbc     p2h, tmp1         			; Subtract it from us
	
	brsh    ast00						; Its less than us, continue walking the list

	add     p2l, tmp0         			; Restore us
	adc     p2h, tmp1
	sub     tmp0, p2l         			; Subtract us from it
	sbc     tmp1, p2h

	std     Z+TmrCount+NextL, tmp0
	std     Z+TmrCount+NextH, tmp1  	; Put it back out and insert us in front.
;
; Wrap up: Z->Current, P1->Tcb, Y->Prev, P2 = Count
; Insert Tcb into chain
;
ast01:

	std     Y+NextH, p1h
	std     Y+NextL, p1l    			; Prev.next = NewTCB
	mov     Yh, p1h
	mov     Yl, p1l
	std     Y+NextH, Zh     			; NewTCB.next = Current
	std     Y+NextL, Zl
	std     Y+TmrCount+NextL, p2l
	std     Y+TmrCount+NextH, p2h 		; NewTCB.Count = count

	;;rjmp    _LeaveISR
	
	_RTOS_EPILOG

	_ENDFUNC	;; _FUNCTION(RTOS_StartTimer)

/******************************************************************************
;; Prototype:	RTOS_TMR_t * RTOS_CancelTimer(RTOS_TMR_t * tmr)
;; Description:	Go through the list looking for timer.  If found, and not at
;;			    end of list, then add count to Next timer.
;; Passed:      p1h:p1l = &(tmr) - pointer to Timer 
;; Returns:		Pointer to removed timer, or 0 if failure
;; Uses:        everithing )
;; Stack:		TBD
;; Calls:		_EnterISR/_LeaveISR, _RemoveObj, RTOS_SetObjectEventISR
;; Assumes:		-
;; Notes:		None
******************************************************************************/
	_FUNCTION(RTOS_CancelTimer)
        
RTOS_CancelTimer:

	;; rcall 	_EnterISR					; RTOS_Prolog
	
	_ENTER_CRITICAL
	
	_RTOS_PROLOG
	
	rcall   RTOS_SetObjectEventISR

	ldd     p2l, Y+_p1l
	ldd     p2h, Y+_p1h
	ldi     Zl, lo8(_RtosTimerQueue)
	ldi     Zh, hi8(_RtosTimerQueue)

	;; BeginCritical
	;; -=AAK022=- cli
	
	_ENTER_CRITICAL

	rcall   _RemoveObj   				; Z = next item, R23:R22 = obj.
	
	subi    tmp0, lo8(0)
	sbci    tmp1, hi8(0)    			; Test if in timer queue
	
	breq    act00           			; No, just return

	mov     Yl, p2l         			; Y is restored in _LeaveISR
	mov     Yh, p2h
	ldd     p2l, Y+TmrCount+NextL
	ldd     p2h, Y+TmrCount+NextH
	ldd     r1h, Z+TmrCount+NextH 		; Next
	ldd     r1l, Z+TmrCount+NextL
	add     r1l, p2l
	adc     r1h, p2h
	std     Z+TmrCount+NextL, r1l
	std     Z+TmrCount+NextH, r1h
	
	;;rjmp    _LeaveISR
	
	_RTOS_EPILOG
	
act00:

	std     Y+_p1l, tmp0      			; Return 0, failure.
	std     Y+_p1h, tmp1
	
	;; rjmp    _LeaveISR
	_RTOS_EPILOG
	
        
    _ENDFUNC	;; _FUNCTION(RTOS_CancelTimer)
	
/******************************************************************************
;; Prototype:	void RTOS_StartTimerMessage(
;;						RTOS_TMS_t * tms, RTOS_U16_t cnt, RTOS_MSQ_t * msq )
;; Description:	Should check and halt if TMS already queued. Resets timer
;;				object event if counter==0 and returns	
;; Passed:      R25:R24 = &(tms) 	pointer to timer message structure
;;              R23:R22 = Timeout	timeout in system timer ticks
;;              R20:R21 = &(queue)	queue to the send the message on 
;;				timeout expired
;; Returns:		none
;; Uses:        Z, Y, tmp0, tmp1
;; Stack:		TBD
;; Calls:		RTOS_SetObjectEvent
;; Assumes:		-
;; Notes:		This functions should check if timer is already queued. If 
;;				timer' counter is zero then timer' event shold be reseted. 
******************************************************************************/
        _FUNCTION(RTOS_StartTimerMessage)

RTOS_StartTimerMessage:

	subi    p2l, lo8(-0)
	sbci    p2h, hi8(-0)

	brne    stm01

	mov     p2l, p1l
	mov     p2h, p1h
	mov     p1l, tmp2               	;; tmp3:tmp2 == P3 for GCC
	mov     p1h, tmp3

	rjmp    RTOS_SendMessage         	;; Count Zero, just queue onto message queue

stm01:

	mov     Zl, p1l
	mov     Zh, p1h
	std     Z+TmrQueue+NextH, tmp3
	std     Z+TmrQueue+NextL, tmp2      ;; Stuff the queue
	ldi     tmp2, lo8(TIMERMESSAGE_EV)
	ldi     tmp3, hi8(TIMERMESSAGE_EV)
	std     Z+TmrEvent+NextH, tmp3
	std     Z+TmrEvent+NextL, tmp2   	;; Cruft up bogus semaphore

	rjmp    CountNotZero            	;; This is in RTOS_StartTimer()

	_ENDFUNC 	;; _FUNCTION(RTOS_StartTimerMessage)
	
/******************************************************************************
;; Prototype:	RTOS_TMS_t * RTOS_CancelTimerMessage(RTOS_TMS_t *, RTOS_MSQ_t * )
;; Description:	Cancel active timer message. Walks trough the list of active 
;;				timers until specified timer is found. If found not end of the 
;;				list, then remove and add count to the next timer in the list.
;;				If not found in timer list, then tries to remove from specified 
;;				message queue.
;; Passed:      p1h:p1l = pointer to the timer message to be removed
;;				p2h:p2l = pominter to accossiated message queue
;; Returns:		p1h:p1l = pointer to removed timer message
;; Uses:        TBD
;; Stack:		TBD
;; Calls:		TBD
;; Assumes:		none
;; Notes:		none
******************************************************************************/
        _FUNCTION(RTOS_CancelTimerMessage)
        
RTOS_CancelTimerMessage:

	_ENTER_CRITICAL
	_RTOS_PROLOG
	
	mov     p2l, p1l
	mov     p2h, p1h
	ldi     Zl, lo8(_RtosTimerQueue)
	ldi     Zh, hi8(_RtosTimerQueue)

	rcall   _RemoveObj		   			;; Z = next item, R23:R22 = obj.
	
	subi    tmp0, lo8(0)
	sbci    tmp1, hi8(0)    			;; Test if in timer queue
	
	breq    rctm00          			;; No, remove from message queue

	mov     Yl, p2l         			;; Y is restored in _Epilog
	mov     Yh, p2h
	ldd     p2l, Y+TmrCount+NextL
	ldd     p2h, Y+TmrCount+NextH
	ldd     r1h, Z+TmrCount+NextH 		;; Next
	ldd     r1l, Z+TmrCount+NextL
	add     r1l, p2l
	adc     r1h, p2h
	std     Z+TmrCount+NextL, r1l
	std     Z+TmrCount+NextH, r1h

	_RTOS_EPILOG
	
	;
	; Timer expired, try to remove from message queue
	;
	
rctm00:

	ldd     Zl, Y+_p2l
	ldd     Zh, Y+_p2h      			;; Recover pointer to queue
	
	rcall   _RemoveObj		   			;; p2 still point to TimerMessage object
	
	subi    tmp0, lo8(0)
	sbci    tmp1, hi8(0)    			;; Test if in message queue
	
	brne    rctm01
	
	std     Y+_r1l, Xl      			;; If not found, stuff 0 into return registers
	std     Y+_r1h, Xh
	
rctm01:

	_RTOS_EPILOG
	
	_ENDFUNC	;; _FUNCTION(RTOS_CancelTimerMessage)

/******************************************************************************
;; Prototype:	void RTOS_TimerHandlerFNC(void)
;; Description:	RTOS system timer interrupt handler. 
;; Passed:      none
;; Returns:		none
;; Uses:        Z, tmp0, tmp1
;; Stack:		TBD
;; Calls:		RTOS_SetObjectEventISR, RTOS_SendMessageISR
;; Assumes:		Timer peripheral module is configured outside to generate this 
;;				interrupt. User points this handler in RTOS interrupt vector 
;;				configuration like follows:
;;
;;					RTOS_DECLARE_ISR(some_timer_vector, RTOS_TimerHandler)
;;
;; Notes:		THIS IS INTERRUPT CODE and must be called within an
;; 				_EnterISR/_LeaveISR section.
;;
******************************************************************************/
        _FUNCTION(RTOS_TimerHandlerFNC)
        
RTOS_TimerHandlerFNC:

        lds     Yh, _RtosTimerQueue+NextH
        lds     Yl, _RtosTimerQueue+NextL
        adiw    Yl, 0
		
        breq    tmi02           			;; Empty queue, return
;
; Y points to the first element in the queue.
;
        ldd     Zh, Y+TmrCount+NextH
        ldd     Zl, Y+TmrCount+NextL        
        sbiw    Zl, 1           			;; Y->Count--
        std     Y+TmrCount+NextH, Zh
        std     Y+TmrCount+NextL, Zl

tmi01:

        or      Zl, Zh          			;; While (Y->Count == 0)
        brne    tmi02           			;; {
        ldd     Xl, Y+NextL
        ldd     Xh, Y+NextH
        sts     _RtosTimerQueue+NextH, Xh 	;; Point queue to (new) first element
        sts     _RtosTimerQueue+NextL, Xl
        std     Y+NextH, Zh     			;;   Zero out link
        std     Y+NextL, Zl
        ldd     p1l, Y+TmrEvent+NextL
        ldd     p1h, Y+TmrEvent+NextH
        subi    p1l, lo8(TIMERMESSAGE_EV)
        sbci    p1h, hi8(TIMERMESSAGE_EV)
        brne    tmi04
        ldd     p1l, Y+TmrQueue+NextL
        ldd     p1h, Y+TmrQueue+NextH
        mov     p2h, Yh
        mov     p2l, Yl

        rcall   RTOS_SendMessageISR

        rjmp    tmi03

tmi04:

        mov     p1l, Yl
        mov     p1h, Yh

        rcall   RTOS_SetObjectEventISR

tmi03:

        adiw    Xl, 0           			;;   If Next Tmr == 0 (list empty)
        breq    tmi02           			;;       break;
											;;   else
        mov     Yl, Xl
        mov     Yh, Xh
        ldd     Zh, Y+TmrCount+NextH 		;;   load up count and loop.
        ldd     Zl, Y+TmrCount+NextL

        rjmp    tmi01           			;; }

tmi02:

	LDS		p1l,_RtosTimeCounter+NextL
	LDS		p1h,_RtosTimeCounter+NextH
	ADIW	p1l,1
	STS		_RtosTimeCounter+NextL,p1l
	STS		_RtosTimeCounter+NextH,p1h
		
    ret

        _ENDFUNC	;; _FUNCTION(RTOS_TimerHandlerFNC)
		
/******************************************************************************
;; Prototype:	RTOS_U16_t	RTOS_GetTime( void )
;;				RTOS_U16_t	RTOS_GetTimeISR( void )
;; Description:	TBD
;; Passed:      none
;; Returns:		p1h:p1l = value of time counter 
;; Uses:        nothing
;; Stack:		TBD
;; Calls:		none
;; Assumes:		none
;; Notes:		none
******************************************************************************/
	_FUNCTION(RTOS_GetTime)
	_PUBLIC(RTOS_GetTimeISR)
	
RTOS_GetTime:
RTOS_GetTimeISR:

	_ENTER_CRITICAL
	
	LDS		p1l,_RtosTimeCounter+NextL
	LDS		p1h,_RtosTimeCounter+NextH
	
	_LEAVE_CRITICAL
		
    ret

	_ENDFUNC 	;; _FUNCTION(RTOS_GetTime)








   
        _END	;; _MODULE(rtos_timer)
